#include "Physics.h"


Physics::Physics(void)
{
}

Physics::~Physics(void)
{
}

void Physics::init(std::list<Particle>* _particles, std::list<ForceGenerator*>* _forceGenerators, Gravity* _gravity)
{
	particles = _particles;
	forceGenerators = _forceGenerators;
	gravity = _gravity;
}

void Physics::addCollisionPlane(LimitedPlane* plane)
{
	collisionPlanes.push_back(plane);
}

void Physics::addSpring(LinearSpring* spring)
{
	springs.push_back(spring);
}

void Physics::update(int delta)
{
	for(std::list<ForceGenerator*>::iterator I = forceGenerators->begin(); I != forceGenerators->end(); ++I)
	{
		(*I)->update(delta);
	}

	for(std::list<LinearSpring*>::iterator I = springs.begin(); I != springs.end(); ++I)
	{
		(*I)->update(delta);
	}

	float dt = (float)delta/1000; //seconds!
	for(std::list<Particle>::iterator P = particles->begin(); P != particles->end(); ++P)
	{
		// physics calculations for each particle
		if(P->ttl > 0)
		{
			//new velocity
			P->velocity += P->force * dt;

			testCollision(&(*P), dt); //check collisions after getting next velocity
			
			//calculate new position
			P->position += P->velocity * dt;
			P->force = Vector3f::zero;
			
			P->ttl -= delta; //time in ms
		}
	}
}

void Physics::testCollision(Particle* particle, float dt) {
	for(std::list<LimitedPlane*>::iterator P = collisionPlanes.begin(); P != collisionPlanes.end(); ++P)
	{
		LimitedPlane* plane = (*P);

		float D = plane->normal.dot( plane->v1 ); //alternative description of a plane
		float distance = plane->normal.dot( particle->position ) - D; // the distance of the particle to the plane

		float alpha = plane->normal.dot(particle->velocity)*distance; //whether or not we are moving toward, paralell to, or away from the plane

		if(alpha < 0)
		{
			float t = -distance / plane->normal.dot(particle->velocity*dt); //calculate time of collision

			if(t >= 0 && t <= 1) {
				float v_length = particle->velocity.length(); //magnitude of velocity (speed)
				Vector3f v_normalized = Vector3f::normalize(particle->velocity); //direction of velocity
				Vector3f collision_normal = plane->normal; //normal used to calculate reflection vector
			
				Vector3f collisionPoint = particle->position + v_normalized*distance; //set position to plane

				//check if collision Point is actually between the start and end point of the plane
				if ( plane->pointIsOnPlane(collisionPoint) ) {

					//particle->color = CL_Colorf::yellow;

					particle->position = collisionPoint; //set collision point as new position

					
					//if velocity is smaller than a specific epsilon, just stop this particle and add it to the friction force generator
					if(v_length < EPS) {
						particle->color = CL_Colorf::red;
						particle->velocity = Vector3f::zero;
						particle->force = Vector3f::zero;
						//gravity->remove(particle);
						plane->frictionGenerator->add(particle);
						break;
					}
					
					//reflection vector:
					//r = 2(n dot v)n-v
					Vector3f reflection = collision_normal*((collision_normal.dot(v_normalized))*2) - v_normalized;
					reflection.normalize();

					particle->velocity = (reflection*-1) * v_length * (1-plane->damping) ; //damped reflection velocity
				}
			}
		}
	}
}

